/**
 * Copyright (C) 2025 ROKAE (Beijing) Technology Co., LTD.
 * All Rights Reserved.
 *
 * Information in this file is the intellectual property of Rokae Technology Co., Ltd,
 * And may contains trade secrets that must be stored and viewed confidentially.
 *
 * @file InspireRobotsRLCmd.cpp
 * @brief 注册 RL 指令
 */
#include "InspireRobotsRLCmd.h"

namespace inspire_robots_cmd
{
    using namespace xcore_api::rl_cmd;
    using namespace xcore_api::endtool;

    /**
     * @brief 实现指令InspireRobotsSetAngle
     */
    MORDERN_CMD_ARGS_API(InspireRobotsSetAngle)
    {
        std::vector<ArgTypeAPI> v;
        // 必选参数，从站的ID
        v.emplace_back(ValueTypeAPI::VALUE_INT, true);
        // 必选参数，各个手指关节的角度
        v.emplace_back(ValueTypeAPI::VALUE_INT, true);
        v.emplace_back(ValueTypeAPI::VALUE_INT, true);
        v.emplace_back(ValueTypeAPI::VALUE_INT, true);
        v.emplace_back(ValueTypeAPI::VALUE_INT, true);
        v.emplace_back(ValueTypeAPI::VALUE_INT, true);
        v.emplace_back(ValueTypeAPI::VALUE_INT, true);
        return RequireArgsAPI(v);
    }

    MORDERN_CMD_EXECUTE_API(InspireRobotsSetAngle)
    {
        cmdExecute_t result = [=]
        {
            auto args = GetCmdArgs();
            // 获取从站ID
            int slave_id = args[0].GetIntVal();
            // 获取各个手指关节的角度
            int finger1_angle = args[1].GetIntVal();
            int finger2_angle = args[2].GetIntVal();
            int finger3_angle = args[3].GetIntVal();
            int finger4_angle = args[4].GetIntVal();
            int finger5_angle = args[5].GetIntVal();
            int finger6_angle = args[6].GetIntVal();

            bool ret = api.startInspireRobots(
                slave_id, finger1_angle, finger2_angle,
                finger3_angle, finger4_angle,
                finger5_angle, finger6_angle);

            if (ret)
            {
                LOG(INFO) << "send inspire robots set angle cmd result: " << ret;
                LOG(INFO) << "slave_id: " << slave_id;
                LOG(INFO) << "finger1_angle: " << finger1_angle;
                LOG(INFO) << "finger2_angle: " << finger2_angle;
                LOG(INFO) << "finger3_angle: " << finger3_angle;
                LOG(INFO) << "finger4_angle: " << finger4_angle;
                LOG(INFO) << "finger5_angle: " << finger5_angle;
                LOG(INFO) << "finger6_angle: " << finger6_angle;

                return CommandProcessResultAPI::CMD_PROCESS_SUCCESS;
            }
            else
            {
                return CommandProcessResultAPI::CMD_PROCESS_ERROR;
            }
        };

        return result;
    }

    std::shared_ptr<ModernRLCmdAPI> makeInspireRobotsSetAngle()
    {
        RLCmdConfigAPI cfg;
        cfg.SetFuncName("InspireRobotsSetAngle");
        // 设置指令的前瞻方式
        cfg.SetLookAhead(CmdLookAheadImplAPI::WAIT_MOVE_PAUSE);
        // 设置指令的任务限制类型
        cfg.SetTaskLimit(TaskLimitAPI::TASK_LIMIT_MOVE);
        // 设置指令是否需要括号
        cfg.SetRequireBracket(false);
        // 设置指令的返回值类型
        cfg.SetReturnType(ValueTypeAPI::VALUE_BOOL);
        // 设置步退行为
        cfg.SetStepBack(StepBackAPI::STEP_BACK_STOP);
        
        return std::make_shared<InspireRobotsSetAngle>(cfg);
    }

    /**
     * @brief 实现指令InspireRobotsSetForce
     */
    MORDERN_CMD_ARGS_API(InspireRobotsSetForce)
    {
        std::vector<ArgTypeAPI> v;
        // 必选参数，从站的ID
        v.emplace_back(ValueTypeAPI::VALUE_INT, true);
        // 必选参数，各个手指关节的力设置
        v.emplace_back(ValueTypeAPI::VALUE_INT, true);
        v.emplace_back(ValueTypeAPI::VALUE_INT, true);
        v.emplace_back(ValueTypeAPI::VALUE_INT, true);
        v.emplace_back(ValueTypeAPI::VALUE_INT, true);
        v.emplace_back(ValueTypeAPI::VALUE_INT, true);
        v.emplace_back(ValueTypeAPI::VALUE_INT, true);
        return RequireArgsAPI(v);
    }

    MORDERN_CMD_EXECUTE_API(InspireRobotsSetForce)
    {
        cmdExecute_t result = [=]
        {
            auto args = GetCmdArgs();
            // 获取从站ID
            int slave_id = args[0].GetIntVal();
            // 获取各个手指关节的力设置
            int finger1_force = args[1].GetIntVal();
            int finger2_force = args[2].GetIntVal();
            int finger3_force = args[3].GetIntVal();
            int finger4_force = args[4].GetIntVal();
            int finger5_force = args[5].GetIntVal();
            int finger6_force = args[6].GetIntVal();

            bool ret = api.setForceInspireRobots(
                slave_id, finger1_force, finger2_force,
                finger3_force, finger4_force,
                finger5_force, finger6_force);

            if (ret)
            {
                LOG(INFO) << "send inspire robots set force cmd result: " << ret;
                LOG(INFO) << "slave_id: " << slave_id;
                LOG(INFO) << "finger1_force: " << finger1_force;
                LOG(INFO) << "finger2_force: " << finger2_force;
                LOG(INFO) << "finger3_force: " << finger3_force;
                LOG(INFO) << "finger4_force: " << finger4_force;
                LOG(INFO) << "finger5_force: " << finger5_force;
                LOG(INFO) << "finger6_force: " << finger6_force;

                return CommandProcessResultAPI::CMD_PROCESS_SUCCESS;
            }
            else
            {
                return CommandProcessResultAPI::CMD_PROCESS_ERROR;
            }
        };

        return result;
    }

    std::shared_ptr<ModernRLCmdAPI> makeInspireRobotsSetForce()
    {
        RLCmdConfigAPI cfg;
        cfg.SetFuncName("InspireRobotsSetForce");
        // 设置指令的前瞻方式
        cfg.SetLookAhead(CmdLookAheadImplAPI::WAIT_MOVE_PAUSE);
        // 设置指令的任务限制类型
        cfg.SetTaskLimit(TaskLimitAPI::TASK_LIMIT_MOVE);
        // 设置指令是否需要括号
        cfg.SetRequireBracket(false);
        // 设置指令的返回值类型
        cfg.SetReturnType(ValueTypeAPI::VALUE_BOOL);
        // 设置步退行为
        cfg.SetStepBack(StepBackAPI::STEP_BACK_STOP);
        
        return std::make_shared<InspireRobotsSetForce>(cfg);
    }

    /**
     * @brief 实现指令InspireRobotsSetSpeed
     */
    MORDERN_CMD_ARGS_API(InspireRobotsSetSpeed)
    {
        std::vector<ArgTypeAPI> v;
        // 必选参数，从站的ID
        v.emplace_back(ValueTypeAPI::VALUE_INT, true);
        // 必选参数，各个手指关节的速度设置
        v.emplace_back(ValueTypeAPI::VALUE_INT, true);
        v.emplace_back(ValueTypeAPI::VALUE_INT, true);
        v.emplace_back(ValueTypeAPI::VALUE_INT, true);
        v.emplace_back(ValueTypeAPI::VALUE_INT, true);
        v.emplace_back(ValueTypeAPI::VALUE_INT, true);
        v.emplace_back(ValueTypeAPI::VALUE_INT, true);
        return RequireArgsAPI(v);
    }
    
    MORDERN_CMD_EXECUTE_API(InspireRobotsSetSpeed)
    {
        cmdExecute_t result = [=]
        {
            auto args = GetCmdArgs();
            // 获取从站ID
            int slave_id = args[0].GetIntVal();
            // 获取各个手指关节的速度设置
            int finger1_speed = args[1].GetIntVal();
            int finger2_speed = args[2].GetIntVal();
            int finger3_speed = args[3].GetIntVal();
            int finger4_speed = args[4].GetIntVal();
            int finger5_speed = args[5].GetIntVal();
            int finger6_speed = args[6].GetIntVal();

            bool ret = api.setSpeedInspireRobots(
                slave_id, finger1_speed, finger2_speed,
                finger3_speed, finger4_speed,
                finger5_speed, finger6_speed);

            if (ret)
            {
                LOG(INFO) << "send inspire robots set speed cmd result: " << ret;
                LOG(INFO) << "slave_id: " << slave_id;
                LOG(INFO) << "finger1_speed: " << finger1_speed;
                LOG(INFO) << "finger2_speed: " << finger2_speed;
                LOG(INFO) << "finger3_speed: " << finger3_speed;
                LOG(INFO) << "finger4_speed: " << finger4_speed;
                LOG(INFO) << "finger5_speed: " << finger5_speed;
                LOG(INFO) << "finger6_speed: " << finger6_speed;

                return CommandProcessResultAPI::CMD_PROCESS_SUCCESS;
            }
            else
            {
                return CommandProcessResultAPI::CMD_PROCESS_ERROR;
            }
        };

        return result;
    }

    std::shared_ptr<ModernRLCmdAPI> makeInspireRobotsSetSpeed()
    {
        RLCmdConfigAPI cfg;
        cfg.SetFuncName("InspireRobotsSetSpeed");
        // 设置指令的前瞻方式
        cfg.SetLookAhead(CmdLookAheadImplAPI::WAIT_MOVE_PAUSE);
        // 设置指令的任务限制类型
        cfg.SetTaskLimit(TaskLimitAPI::TASK_LIMIT_MOVE);
        // 设置指令是否需要括号
        cfg.SetRequireBracket(false);
        // 设置指令的返回值类型
        cfg.SetReturnType(ValueTypeAPI::VALUE_BOOL);
        // 设置步退行为
        cfg.SetStepBack(StepBackAPI::STEP_BACK_STOP);
        
        return std::make_shared<InspireRobotsSetSpeed>(cfg);
    }

    /**
     * @brief 实现指令InspireRobotsGetAngle
     */
    MORDERN_CMD_ARGS_API(InspireRobotsGetAngle)
    {
        std::vector<ArgTypeAPI> v;
        // 必选参数，从站的ID
        v.emplace_back(ValueTypeAPI::VALUE_INT, true);
        // 输出参数，各个手指关节的角度
        v.emplace_back(ValueTypeAPI::VALUE_INT, false);
        v.emplace_back(ValueTypeAPI::VALUE_INT, false);
        v.emplace_back(ValueTypeAPI::VALUE_INT, false);
        v.emplace_back(ValueTypeAPI::VALUE_INT, false);
        v.emplace_back(ValueTypeAPI::VALUE_INT, false);
        v.emplace_back(ValueTypeAPI::VALUE_INT, false);
        return RequireArgsAPI(v);
    }

    MORDERN_CMD_EXECUTE_API(InspireRobotsGetAngle)
    {
        cmdExecute_t result = [=]
        {
            auto args = GetCmdArgs();
            // 获取从站ID
            int slave_id = args[0].GetIntVal();

            int finger1_angle = 0;
            int finger2_angle = 0;
            int finger3_angle = 0;
            int finger4_angle = 0;
            int finger5_angle = 0;
            int finger6_angle = 0;

            bool ret = api.getAngleInspireRobots(
                slave_id, finger1_angle, finger2_angle,
                finger3_angle, finger4_angle,
                finger5_angle, finger6_angle);

            if (ret)
            {
                LOG(INFO) << "get inspire robots angle cmd result: " << ret;
                LOG(INFO) << "slave_id: " << slave_id;
                LOG(INFO) << "finger1_angle: " << finger1_angle;
                LOG(INFO) << "finger2_angle: " << finger2_angle;
                LOG(INFO) << "finger3_angle: " << finger3_angle;
                LOG(INFO) << "finger4_angle: " << finger4_angle;
                LOG(INFO) << "finger5_angle: " << finger5_angle;
                LOG(INFO) << "finger6_angle: " << finger6_angle;

                // 设置输出参数
                args[1].ResetInt(finger1_angle);
                args[2].ResetInt(finger2_angle);
                args[3].ResetInt(finger3_angle);
                args[4].ResetInt(finger4_angle);
                args[5].ResetInt(finger5_angle);
                args[6].ResetInt(finger6_angle);

                return CommandProcessResultAPI::CMD_PROCESS_SUCCESS;
            }
            else
            {
                return CommandProcessResultAPI::CMD_PROCESS_ERROR;
            }
        };

        return result;
    }

    std::shared_ptr<ModernRLCmdAPI> makeInspireRobotsGetAngle()
    {
        RLCmdConfigAPI cfg;
        cfg.SetFuncName("InspireRobotsGetAngle");
        // 设置指令的前瞻方式
        cfg.SetLookAhead(CmdLookAheadImplAPI::ACTIVATE_AT_LOOKAHEAD);
        // 设置指令的任务限制类型
        cfg.SetTaskLimit(TaskLimitAPI::TASK_LIMIT_MOVE);
        // 设置指令是否需要括号
        cfg.SetRequireBracket(false);
        // 设置指令的返回值类型
        cfg.SetReturnType(ValueTypeAPI::VALUE_BOOL);
        // 设置步退行为
        cfg.SetStepBack(StepBackAPI::STEP_BACK_STOP);
        
        return std::make_shared<InspireRobotsGetAngle>(cfg);
    }
}

/**
 * @brief 注册 Inspire Robots 相关指令
 */
void RegisterInspireRobotsRLCmd()
{
    // 注册 Inspire Robots 设置角度指令
    xcore_api::rl_cmd::RegPluginCmdsAPI(
        inspire_robots_cmd::makeInspireRobotsSetAngle());

    // 注册 Inspire Robots 设置力指令
    xcore_api::rl_cmd::RegPluginCmdsAPI(
        inspire_robots_cmd::makeInspireRobotsSetForce());

    // 注册 Inspire Robots 设置速度指令
    xcore_api::rl_cmd::RegPluginCmdsAPI(
        inspire_robots_cmd::makeInspireRobotsSetSpeed());

    // 注册 Inspire Robots 获取角度指令
    xcore_api::rl_cmd::RegPluginCmdsAPI(
        inspire_robots_cmd::makeInspireRobotsGetAngle());
}